
#include <unistd.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "driver/uart.h"
#include "string.h"
#include "driver/gpio.h"
#include "Custom_Config.h"

#include "cstm_gpio.h"
#include "steering_motor.h"

int steering_motor_init() {
    // gpio init
    init_gpio_output(IO3);
    return 0;
}

int steering_motor_run(int step, bool clockwise) {
    for (int i = 0; i < 6; i++) {
        for(int j=0; j<100; j++)
        {
            int high_us = 500 * i;
            set_gpio(IO3, 1);
            // vTaskDelay(6/ portTICK_PERIOD_MS);
            usleep(high_us); // have test most quickly 400
            set_gpio(IO3, 0);
            usleep(20*1000 - high_us);
        }
    }
    return 0;
}
